#include "Servo.h"

double ccr;
uint8_t outlimit_flag = 0;
uint8_t Position_state[5]={0,0,0,0,0};
RobotAxis axis;
void Robot_jointAxis_SetAngle(double now_angles[]);

/**
 * @brief 机械臂关节舵机初始化
 * @param dimensions 无
 * @return 无
 */
void Servo_Init(void)
{
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);

}

void RobotAxis_Init(double now_angles[])
{
  Robot_jointAxis_SetAngle(now_angles);
}

/**
 * @brief 单独设置机械臂关节舵机角度
 * @param axis 关节名
 * @param angle 角度
 * @return 无
 */
void Servo_SetAngle(RobotAxis axis,double angle) 
{
  ccr = 500 + (double)(angle * 2000 / 180);//0°-180°的对应打角是0.5ms-2.5ms   (ccr/arr)*20ms = [(500+180*200/180)/20000] * 20ms = 2.5ms
  switch (axis)
  {
    case AXIS_BASE_ROTATE:
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, ccr);
    last_angles[0] = angle;
      break;
    
    case AXIS_SHOULDER_PITCH:
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, ccr);
    last_angles[1] = angle;
      break;
    
    case AXIS_ELBOW_PITCH:
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, ccr);
    last_angles[2] = angle;
    break;

    case AXIS_WRIST_PITCH:
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, ccr);
    last_angles[3] = angle;
    break;

    case AXIS_WRIST_ROTATE:
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, ccr);
    last_angles[4] = angle;
    break;

    case AXIS_GRIPPER_ROTATE:
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, ccr);
    now_temp_angles[5] = angle;
    last_angles[5] = angle;
    break;
    }

}

/**
 * @brief 调试舵机角度
 * @return 无
 */
void Servo_Test(void)
{
  
  
}

/**
 * @brief 关节剩余变化角度
 * @param *jointangles 存储角度变化的结构体
 * @param last_angles[] 上一次关节角度数组
 * @param theta_angles[] 关节变化角度数组
 * @return 无
 */
void Axis_remaining_bais(JointAngles *jointangles,double last_angles[],double theta_angles[])
{
    for(uint8_t i=0;i<Axis_num;i++)
    {
      switch (i)
      {
      case 0:
          theta_angles[i] = home_angles[i] + jointangles->theta1 - last_angles[i];
          if(fabsf(theta_angles[i]) <= 2)
          {
              theta_angles[i]=0;
              Position_state[i]=1;
          }
          break;
      case 1:
          theta_angles[i] = home_angles[i] + jointangles->theta2 - last_angles[i];
          if(fabsf(theta_angles[i]) <= 2)
          {
              theta_angles[i]=0;
              Position_state[i]=1;
          } Position_state[i]=1;
          
          break;    
      case 2:
          theta_angles[i] = home_angles[i] + jointangles->theta3 - last_angles[i];
          if(fabsf(theta_angles[i]) <= 2)
          {
              theta_angles[i]=0;
              Position_state[i]=1;
          }
          break;
      case 3:
          theta_angles[i] = home_angles[i] + jointangles->theta4 - last_angles[i];;
          if(fabsf(theta_angles[i]) <= 2)
          {
              theta_angles[i]=0;
              Position_state[i]=1;
          }
          break;
      case 4:
          theta_angles[i] = home_angles[i] + jointangles->theta5 - last_angles[i];;
          if(fabsf(theta_angles[i]) <= 2)
          {
              theta_angles[i]=0;
              Position_state[i]=1;
          }
          break;
      
      }
        
    }
}

/**
 * @brief 关节是否成功到达指定位置的函数
 * @param Position_state[] 记录关节到达指定位置的数组
 * @return 1为全部到达，0为部分到达
 */
uint8_t Position_Succes(uint8_t Position_state[])
{
    uint8_t temp = Position_state[0] + Position_state[1] + Position_state[2] + Position_state[3] + Position_state[4];
    if(temp == 5)
    {
      return 1;
    }
    return 0;
}

/**
 * @brief 关节到达位置标志位清零的函数
 * @param dimensions 无
 * @return 无
 */
void Position_Clear(void)
{
  for (uint8_t i = 0; i < Axis_num; i++)
  {
    Position_state[i] = 0;
  }
}

/**
 * @brief 合并设置机械臂关节舵机角度
 * @param now_angles[] 限幅后的关节角度数组
 * @return 无
 */
void Robot_jointAxis_SetAngle(double now_angles[])
{
  for (uint8_t i = 0; i < Axis_num; i++)
  {
    switch (i)
    {
      case 0:
        axis = AXIS_BASE_ROTATE;
          break;
      case 1:
        axis = AXIS_SHOULDER_PITCH;
          break;    
      case 2:
        axis = AXIS_ELBOW_PITCH;
          break;
      case 3:
        axis = AXIS_WRIST_PITCH; 
          break;
      case 4:
        axis = AXIS_WRIST_ROTATE;
          break;
    }
    Servo_SetAngle(axis,now_angles[i]);

    // for(uint8_t i=0;i<Axis_num;i++)
    // {
    //   last_angles[i] = now_angles[i];
    // }
  }

}


/**
 * @brief 关节速度限制
 * @param steer_out[] PID输出的角度变化值
 * @param limit_angles[] 限幅后的舵机打角值
 * @param limit 最大变化角度限制值
 * @return 无
 */
void Axis_Speed_Limit(double steer_out[], double limit_angles[], float limit)
{
      for(uint8_t i=0;i<Axis_num;i++)
      {
        if (fabsf(steer_out[i]) > limit)
        {
          
          if(steer_out[i]>0){limit_angles[i] = limit;}
          else{limit_angles[i] = -limit;}
        }
        else
        {
          limit_angles[i] = -steer_out[i];
        }
      }

}

/**
 * @brief 更新当前关节角度函数
 * @param now_temp_angles[] 当前舵机的临时角度数组
 * @param last_angles[] 上一次舵机处于的角度值数组
 * @param limit_angles[] 限幅后的舵机打角增值数组
 * @return 无
 */
void Axis_now_angle_updata(double now_temp_angles[],double last_angles[],double limit_angles[])
{
  for(uint8_t i=0;i<Axis_num;i++)
  {
    now_temp_angles[i] = last_angles[i] + limit_angles[i];
  }
}



/**
 * @brief 判断关节变化角度有没有超限
 * @param dimensions 当前角度的数组
 * @return 1为正常，0为超限
 */
uint8_t Axis_angle_limit(double now_angles[])
{
  for (uint8_t i = 0; i < Axis_num; i++)
  {
    if(now_angles[i]<Axis_Angle_Min)
    {
      now_angles[i] = Axis_Angle_Min;
      outlimit_flag = 1;
    }
    else if (now_angles[i]>Axis_Angle_Max)
    {
      now_angles[i] = Axis_Angle_Max;
      outlimit_flag = 1;
    }

  }

  if(outlimit_flag)
  {
    outlimit_flag = 0;
    return 0;
  }

  return 1;
}

/**
 * @brief 夹爪释放物体
 * @param dimensions 无
 * @return 无
 */
void Gripper_Open(void)
{
  Servo_SetAngle(AXIS_GRIPPER_ROTATE,100);

}

/**
 * @brief 夹爪抓取物体
 * @param angle 抓取角度
 * @return 无
 */
void Gripper_Close(double angle)
{
  if(angle < GRIPPER_Angle_Min)
  {
    angle = GRIPPER_Angle_Min;
  }

  Servo_SetAngle(AXIS_GRIPPER_ROTATE,angle);
}

/**
 * @brief 夹爪角度限幅
 * @param angle 夹爪角度
 * @return 无
 */
void Gripper_Angle_Limit_Control(double angle)
{
  if(angle < GRIPPER_Angle_Min)
  {
    angle = GRIPPER_Angle_Min;
  }
  else if (angle > GRIPPER_Angle_Max)
  {
    angle = GRIPPER_Angle_Max;
  }
  Servo_SetAngle(AXIS_GRIPPER_ROTATE,angle);
}

/**
 * @brief 定时控制夹爪的状态
 * @param timeout 定时时间
 * @param Kinematics_state 夹爪被控制的状态
 * @return 无
 */

void Timer_gripper_control(uint64_t timeout,KinematicsState Kinematics_state)
{
  GripperCommand cmd = { .timeout = timeout, .state = Kinematics_state };
  xQueueSend(xGripperQueue, &cmd, pdMS_TO_TICKS(100));  // 发送命令
}
